from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription,DeclareLaunchArgument
from launch_ros.actions import Node, LifecycleNode
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch.substitutions import LaunchConfiguration
import os

def generate_launch_description():

    # # can_decode
    can_launch_dir = get_package_share_directory('can_decode')
    can_launch_file = os.path.join(can_launch_dir, 'launch', 'can.launch.py')
    can_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(can_launch_file),
    )
    # RTK Node
    rtk_serial_arg =DeclareLaunchArgument(
        "RTK_serial_port",
        default_value="/dev/ttyUSB0",
        description="RTK serial port device"
    )
    RTK_serial_port = LaunchConfiguration("RTK_serial_port")
    read_rtk_node = Node(
        package="rtk",
        namespace="ugv",
        executable="read_rtk",
        name="read_rtk",
        parameters=[{
            "RTK_serial_port":RTK_serial_port
        }]
    )
    # RTK transform Node
    rtk_transform_node = Node(
        package="rtk",
        namespace="ugv",
        executable="lla2local",
        name="lla2local",
    )
    # UGV TF Node
    ugv_tf_node = Node(
        package="sls_ugv_tf",
        namespace="ugv",
        executable="ugv_tf_node",
        name="ugv_tf_node",
    )
    return LaunchDescription([
        can_launch,
        rtk_serial_arg,
        read_rtk_node,
        rtk_transform_node,
        ugv_tf_node
    ])
